#include "funcdef.h"


const Uint8 NMEA_GxGSA_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x02,0x37};
const Uint8 NMEA_GxGSA_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x31};

const Uint8 NMEA_GxGGA_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x29};
const Uint8 NMEA_GxGGA_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x23};

const Uint8 NMEA_GxGSV_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x03,0x3E};
const Uint8 NMEA_GxGSV_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x38};

const Uint8 NMEA_GxRMC_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x04,0x01,0x00,0x00,0x00,0x00,0x00,0x04,0x45};
const Uint8 NMEA_GxRMC_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x3F};

const Uint8 NMEA_GxVTG_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x05,0x01,0x00,0x00,0x00,0x00,0x00,0x05,0x4C};
const Uint8 NMEA_GxVTG_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x05,0x00,0x00,0x00,0x00,0x00,0x00,0x04,0x46};

const Uint8 NMEA_GxGLL_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x01,0x30};
const Uint8 NMEA_GxGLL_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x2A};

const Uint8 NMEA_GxZDA_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x08,0x01,0x00,0x00,0x00,0x00,0x00,0x08,0x61};
const Uint8 NMEA_GxZDA_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x5B};

const Uint8 RXM_RAW_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0x02,0x10,0x01,0x00,0x00,0x00,0x00,0x00,0x22,0x29};
const Uint8 RXM_RAW_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0x02,0x10,0x00,0x00,0x00,0x00,0x00,0x00,0x21,0x23};

const Uint8 NAV_SOL_ON[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x06,0x01,0x00,0x00,0x00,0x00,0x00,0x17,0xDB};
const Uint8 NAV_SOL_OFF[16] = {0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x16,0xD5};

const Uint8 UBX_CFG_CFG[21] = {0xB5,0x62,0x06,0x09,0x0D,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x1C,0xAA};
const Uint8 UBX_CFG_RATE_5HZ[14] = {0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A};
const Uint8 UBX_CFG_RATE_2HZ[14] = {0xB5,0x62,0x06,0x08,0x06,0x00,0xF4,0x01,0x01,0x00,0x01,0x00,0x0B,0x77};
const Uint8 UBX_CFG_RATE_1HZ[14] = {0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39};




#define GPS_MAX_BUFFER_SIZE   256
#pragma DATA_ALIGN(GPS_TxBuffer, GPS_MAX_BUFFER_SIZE);
static Uint8 GPS_TxBuffer[GPS_MAX_BUFFER_SIZE];


static int GPS_init(TSK_Ctrl_b* tsk);

void GPS_update_tsk(void){
	int error = 0;
	Uint8 data[2];
	Uint16 GPS_avail_num = 0;
	TSK_Ctrl_b *self = &(SCB.TCB[TSK_GPS]);

	//data structure initialization
	vNavData.gps_data.length = 0;
	bzeros((unsigned char*)&(vNavData.gps_data.data), GPS_MAX_DATA_SIZE);


	SEM_pend(&GPS_sem, SYS_FOREVER);
	while(self->status != INITIALISED){
		error = GPS_init(self);
		if(error){
			LOG_printf(self->trace, "GPS INIT failed: %d\n", error);
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			//DEBUG
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
		}
	}

	//benchmark
	vTskTime(self,RESET_TIME);
	/* Initialize task's STS object to current time */
	TSK_settime(TSK_self());
	/* Sensor initialized, perform sampling operations */
	for(;;){
		SEM_pend(&GPS_sem, SYS_FOREVER);
		//vTskTime(self,SET_PREV_TIME);
    	//-------------------Get datas from GPS-----------------------------------
    	if(GPS_avail_num == 0){

			error = GPS_i2cRead(self->inhd,self->outhd,GPS_BYTE_NUMBER_MSB,data,2);
			if(error){
				//DEBUG
				LOG_printf(self->trace, "GPS_i2cRead GPS_BYTE_NUMBER_MSB Error: %d",error);
			}else{
				GPS_avail_num = (Uint16)(data[0]) + data[1];
			}
    	}
		if(GPS_avail_num != 0)
		{
        	vNavData.gps_data.length = GPS_avail_num;
			error = GPS_i2cRead(self->inhd, self->outhd, GPS_DATA_STREAM, vNavData.gps_data.data, GPS_avail_num);
			if(error){
				//DEBUG
				LOG_printf(self->trace, "GPS_i2cRead vNavData.gps_data_STREAM Error: %d",error);
				error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
				//DEBUG
				if(error){
					LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
				}else{
					LOG_printf(self->trace, "RESET IIC succeed\n");
				}
			}else{
				error = GPS_process_data(&(vNavData.GPS_BUF),&(vNavData.gps_data));
				if(error){
					LOG_printf(self->trace, "GPS process data error: %d\n", error);
				}
			}
        	GPS_avail_num = 0;
		}
		vTskTime(self,DELTA_TIME);
		/* Get time difference and
		add it to task's STS object */
		TSK_deltatime(TSK_self());
		TSK_yield();
	}

}




static int GPS_init(TSK_Ctrl_b* tsk){
	int status = 0;
	size_t msglen = 0;
	GIO_Handle outhd = tsk->outhd;

	//---------------------Initial GPS-----------------------------------
	//NMEA_GxGGA_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxGGA_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxGGA_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxGSA_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxGSA_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxGSA_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxGSV_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxGSV_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxGSV_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxRMC_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxRMC_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxRMC_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxVTG_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxVTG_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxVTG_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxGLL_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxGLL_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxGLL_OFF failed: %d",status);
		return status;
	}

	//NMEA_GxZDA_OFF
	msglen = 16;
	memcpy(GPS_TxBuffer, NMEA_GxZDA_OFF, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NMEA_GxZDA_OFF failed: %d",status);
		return status;
	}

	//NAV_SOL_ON
	msglen = 16;
	memcpy(GPS_TxBuffer, NAV_SOL_ON, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:NAV_SOL_ON failed: %d",status);
		return status;
	}

	//UBX_CFG_RATE_1HZ
	msglen = 14;
	memcpy(GPS_TxBuffer, UBX_CFG_RATE_1HZ, msglen);
	status = GPS_i2cWrite(outhd, GPS_TxBuffer, msglen);
	if(status){
		//DEBUG
		LOG_printf(tsk->trace, "GPS_init:UBX_CFG_RATE_1HZ failed: %d",status);
		return status;
	}

	//--------------------END Initialization------------------------------
	tsk->status = INITIALISED;
	//DEBUG
	LOG_printf(tsk->trace, "GPS Initialized Successfully\n");
	return status;
}
